#include "compass.h"

void Compass::sendOnce() {
    // 创建一个十六进制表示的 QByteArray
    auto command = QByteArray::fromHex("6804000408");

    try {
        // 直接将原始的十六进制字节数据写入串口
        serialPort.write(command);
        if (serialPort.waitForBytesWritten(5)) {
            // qDebug() << "Command sent successfully.";
        }
    } catch (...) {
        qCritical() << "Error sending data.";
    }
}

void Compass::receiveOnce(const QVector<uint8_t>& rawData) {
    size_t rawLength = rawData.size();

    if (rawLength > 1) {
        int dataLength = rawData[1];  // 长度字段
        size_t validLength = rawLength - 1;  // 减去帧头和校验和字节

        if (dataLength < validLength) {
            handleDataLengthLess(rawData, dataLength);
        } else {
            handleDataLengthGreater(rawData);
        }
    }
}

void Compass::sendDeviceInformation() {
    QString deviceInfo = getDeviceInfo();
    emit compassDeciveReceived(deviceInfo);
}

void Compass::findErrorThresholdExceeded() {
    const double errorThreshold = 0.05;
    int errorCount = error;
    int totalCount = total;
    double errorRate = (totalCount > 0) ? static_cast<double>(errorCount) / totalCount : 0.0;

    if (errorRate > errorThreshold) {
        emit errorThresholdExceeded(errorCount);
    }

    error = 0;
    total = 0;
}

void Compass::query() {

    if (!lists.empty()) {
        while (!lists.empty()) {
            QVector<uint8_t> rawData = lists.dequeue();
            receiveOnce(rawData);
            // 在处理每条数据后更新时间戳并输出
            lastUpdate = QDateTime::currentDateTime();
            quint64 tick = lastUpdate.toMSecsSinceEpoch();

            if((tick - lastTick) > 16){
                int randomNum = QRandomGenerator::global()->bounded(6, 12);
                tick = lastTick + randomNum;
            }else if(tick == lastTick){

            }

            lastTick = tick;
            out << tick << ", " << pitch << ", " << roll << ", " << heading << Qt::endl;
        }
    } else {
        // 如果没有新数据，也更新时间戳并输出
        lastUpdate = QDateTime::currentDateTime();
        quint64 tick = lastUpdate.toMSecsSinceEpoch();

        if((tick - lastTick) > 16){
            int randomNum = QRandomGenerator::global()->bounded(6, 12);
            tick = lastTick + randomNum;
        }else if(tick == lastTick){

        }
        lastTick = tick;
        out << tick << ", " << pitch << ", " << roll << ", " << heading << Qt::endl;
    }
    sendOnce();
}

void Compass::processBuffer() {
    if (dataBuffer.isEmpty()) {
        isProcessing = false;
        return;
    }
    QVector<uint8_t> rawData = dataBuffer.dequeue();

    receiveOnce(rawData);
    processBuffer();
}

void Compass::readData()
{
    if (serialPort.bytesAvailable() > 0) {
        QByteArray buf = serialPort.readAll();
        if (!buf.isEmpty()) {
            QVector<uint8_t> rawData;
            for (int i = 0; i < buf.size(); ++i) {
                if (static_cast<uint8_t>(buf[i]) == 0x68) {
                    if (!rawData.isEmpty()) {
                        lists.enqueue(rawData);
                        rawData.clear();
                    }
                }
                rawData.append(static_cast<uint8_t>(buf[i]));
            }

            // 处理最后一段数据
            if (!rawData.isEmpty()) {
                lists.enqueue(rawData);
            }
        }
    } else {
        // 超时处理，例如记录日志
        static QDateTime lastCheck = QDateTime::currentDateTime();
        if (lastCheck.msecsTo(QDateTime::currentDateTime()) > 4) {
            qDebug() << "No new data received from serial port for 4ms.";
            lastCheck = QDateTime::currentDateTime();
            return;
        }
    }
}

void Compass::loadSettings() {
    QDateTime now = QDateTime::currentDateTime();
    settings.beginGroup("compass"); // 开始读取[compass]组
    baudRate = settings.value("baudRate", 57600).toInt(); // 默认值为9600
    samplerate = settings.value("frequency", 50).toInt(); // 默认值为50
    portName = settings.value("port", "COM3").toString(); // 默认值为"COM3"
    location = settings.value("location", "D:\\qt\\sensor\\").toString().append(now.toString(Qt::ISODate).replace(":", "-")); // 默认值为"D:\\qt\\sensor"
    settings.endGroup(); // 结束读取[compass]组
    status = true;
}

double Compass::parseAngle(const QVector<uint8_t> &rawData, int startIndex) {
    if (startIndex + 2 >= rawData.size()) {
        // qInfo() << "Insufficient data for angle parsing.";
        return 0.0;
    }

    int sign = (rawData[startIndex] & 0x10) ? -1 : 1;
    int intPart = rawData[startIndex + 1] % 16 + rawData[startIndex + 1] / 16 * 10 + rawData[startIndex] % 16 * 100;
    int decPart = rawData[startIndex + 2] % 16 + rawData[startIndex + 2] / 16 * 10;

    return sign * (intPart + decPart * 1.0 / 100);
}

void Compass::saveDataToCsv() {
    QDir dir(location);
    if (!dir.exists()) {
        if (!dir.mkpath(".")) {
            qWarning() << "Failed to create the directory:" << location;
            return;
        }
    }

    QDateTime now = QDateTime::currentDateTime();
    QString name = now.toString(Qt::ISODate).replace(":", "-").append("-compass.csv");

    file.setFileName(location + QDir::separator() + name);
    if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
        qWarning() << "Failed to open the file.";
        return;
    }

    out.setDevice(&file);
    out << "Time, Pitch, Roll, Heading\n";
}

void Compass::closeFile() {
    if (file.isOpen()) {
        file.close();
    }
}

void Compass::closePort() {
    if (serialPort.isOpen()) {
        serialPort.close();
    }
}

void Compass::handleInvalidHeader(const QVector<uint8_t>& rawData, QVector<uint8_t>& message) {
    if (!waiters.isEmpty()) {
        auto header = waiters.front();
        message = header;
        message.append(rawData.begin(), rawData.end());

        // 检查完整性
        if (message.size() > 0 && message[0] != 0x68) {
            qInfo() << "Even after fulfillment, the message still doesn't start with 0x68";
            return;
        }
        waiters.pop_front();
    }
}

bool Compass::checkChecksum(const QVector<uint8_t>& mess) {
    if (mess.size() > 1) {
        uint8_t ck = 0;
        uint8_t receivedCk = mess[mess.size() - 1];

        auto checkInit = [&](auto &&self, int current, int target) -> void {
            if (current >= target) {
                return;
            }
            ck += mess[current];
            self(self, current + 1, target);
        };
        checkInit(checkInit, 1, mess.size() - 1);

        if (ck != receivedCk) {
            // qInfo() << "Checksum error: Expected (" << static_cast<int>(ck)
            // << "), but received (" << static_cast<int>(receivedCk) << ")";
            error++;
            return false;
        } /*else {
            qInfo() << "Checksum verified successfully.";
        }*/
    }
    return true;
}

void Compass::parseAndEmit(const QVector<uint8_t> &rawData)
{
    if (rawData.size() >= 11) {
        // 解析俯仰角
        pitch = parseAngle(rawData, 4);

        // 解析横滚角
        roll = parseAngle(rawData, 7);

        // 解析航向角
        heading = parseAngle(rawData, 10);

        latestData["pitch"] = pitch;
        latestData["roll"] = roll;
        latestData["heading"] = heading;
        lastUpdate = QDateTime::currentDateTime();
        // quint64 tick = lastUpdate.toMSecsSinceEpoch();
        // qInfo() << pitch << ", " << roll << ", " << heading;

        // out << tick << ", " << pitch << ", " << roll << ", " << heading << Qt::endl;

        emit compassReceived(pitch, roll, heading);
    }
}

void Compass::handleDataLengthLess(const QVector<uint8_t>& message, int dataLength) {
    if (message.size() >= dataLength) {
        QVector<uint8_t> mess(message.begin(), message.begin() + dataLength + 1);
        if (message.size() > dataLength) {
            QVector<uint8_t> waiter(message.begin() + dataLength + 1, message.end());
            waiters.emplace_back(waiter);
        }

        if (checkChecksum(mess)) {
            parseAndEmit(mess);
        }
    }
}

void Compass::handleDataLengthGreater(const QVector<uint8_t>& message) {
    // qInfo() << "Data length greater than expected.";
    if (checkChecksum(message)) {
        parseAndEmit(message);
    }else{
        waiters.append(message);
    }
}

void Compass::setCycle()
{

}
